#include "MPU6050.h"
#include "MPU6050_Reg.h"

//* 对接硬件层 开始 *//
#include "free_sw_i2c.h"
#include "main.h"
int mpu6050_hal_i2c_read(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len)
{
	return sw_i2c_read(dev_addr, reg_addr, data, len);
}
int mpu6050_hal_i2c_write(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len)
{
	return sw_i2c_write(dev_addr, reg_addr, data, len);
}
void mpu6050_hal_delay_ms(uint32_t ms)
{
	HAL_Delay(ms);
}
//* 对接硬件层 结束 *//

/// @brief
/// @param handle MPU6050句柄结构体指针
/// @param accel_fs 范围：0:±2g  1:±4g  2:±8g  3:±16g
/// @param gyro_fs  范围：0:±250°/s  1:±500°/s  2:±1000°/s  3:±2000°/s
void MPU6050_Init(MPU6050_Handle *handle, uint8_t accel_fs, uint8_t gyro_fs)
{
	memset(handle, 0, sizeof(MPU6050_Handle));
	MahonyInit(&handle->filter, 20, 0.1);
	handle->accel_full_scale = 2 * (1 << accel_fs);							// 设置加速度计满量程
	handle->gyro_full_scale = 250 * (1 << gyro_fs);							// 设置陀螺仪满量程
	handle->accel_scale = handle->accel_full_scale / 32768;					// 计算加速度计的比例因子
	handle->gyro_scale = handle->gyro_full_scale / 32768 * (M_PI / 180.0f); // 计算陀螺仪的比例因子
	MPU6050_WriteReg(MPU6050_GYRO_CONFIG, gyro_fs << 3);					// 陀螺仪配置寄存器
	MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, accel_fs << 3);					// 加速度计配置寄存器

	/*MPU6050寄存器初始化，需要对照MPU6050手册的寄存器描述配置，此处仅配置了部分重要的寄存器*/
	MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01); // 电源管理寄存器1，取消睡眠模式，选择时钟源为X轴陀螺仪
	MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00); // 电源管理寄存器2，保持默认值0，所有轴均不待机
	MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09); // 采样率分频寄存器，配置采样率
	MPU6050_WriteReg(MPU6050_CONFIG, 0x06);		// 配置寄存器，配置DLPF
}
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{
	mpu6050_hal_i2c_write(MPU6050_ADDRESS, RegAddress, &Data, 1); // 调用底层I2C写函数，写入指定寄存器的数据
}

uint8_t MPU6050_ReadReg(uint8_t RegAddress)
{
	uint8_t Data;
	mpu6050_hal_i2c_read(MPU6050_ADDRESS, RegAddress, &Data, 1); // 调用底层I2C读函数，读取指定寄存器的数据
	return Data;
}
uint8_t MPU6050_GetID(void)
{
	return MPU6050_ReadReg(MPU6050_WHO_AM_I); // 返回WHO_AM_I寄存器的值
}

void MPU6050_GetRawData(MPU6050_RawData *raw)
{
	uint8_t data[14];
	mpu6050_hal_i2c_read(MPU6050_ADDRESS, MPU6050_ACCEL_XOUT_H, data, 14); // 连续读取14个寄存器的数据
	raw->accel_x = (data[0] << 8) | data[1];
	raw->accel_y = (data[2] << 8) | data[3];
	raw->accel_z = (data[4] << 8) | data[5];
	raw->temp = (data[6] << 8) | data[7];
	raw->gyro_x = (data[8] << 8) | data[9];
	raw->gyro_y = (data[10] << 8) | data[11];
	raw->gyro_z = (data[12] << 8) | data[13];
}
/// @brief 校准MPU6050
/// @note  校准MPU6050，需要在静止状态下进行，采样一定数量的数据，计算平均值作为偏移值
/// @param handle
void MPU6050_Calibrate(MPU6050_Handle *handle)
{
	MPU6050_RawData raw;
	int samples = 1000; // 采样次数
	int32_t ax_sum = 0, ay_sum = 0, az_sum = 0;
	int32_t gx_sum = 0, gy_sum = 0, gz_sum = 0;

	mpu6050_hal_delay_ms(1000);

	for (int i = 0; i < samples; i++)
	{
		MPU6050_GetRawData(&raw); // 读取原始数据
		ax_sum += raw.accel_x;
		ay_sum += raw.accel_y;
		az_sum += raw.accel_z;
		gx_sum += raw.gyro_x;
		gy_sum += raw.gyro_y;
		gz_sum += raw.gyro_z;
		mpu6050_hal_delay_ms(1);
	}
	handle->gyro_offset_x = gx_sum / samples;
	handle->gyro_offset_y = gy_sum / samples;
	handle->gyro_offset_z = gz_sum / samples;
}
void MPU6050_GetProcessed(MPU6050_Handle *handle, float dt)
{
	MPU6050_RawData raw;
	MPU6050_GetRawData(&raw); // 读取原始数据
	handle->sensor_data.ax = handle->accel_scale * (raw.accel_x);
	handle->sensor_data.ay = handle->accel_scale * (raw.accel_y);
	handle->sensor_data.az = handle->accel_scale * (raw.accel_z);
	handle->temp = 36.53f + (1.0f / 340.0f) * (raw.temp);
	handle->sensor_data.gx = handle->gyro_scale * (raw.gyro_x - handle->gyro_offset_x);
	handle->sensor_data.gy = handle->gyro_scale * (raw.gyro_y - handle->gyro_offset_y);
	handle->sensor_data.gz = handle->gyro_scale * (raw.gyro_z - handle->gyro_offset_z);
	MahonyUpdateIMU(&handle->filter, &handle->sensor_data, dt);
	QuaternionToEuler(&handle->filter, &handle->euler_angles);
}

/**
 * @brief 初始化Mahony滤波器
 * @param filter 滤波器实例指针
 * @param Kp 比例增益（推荐10~50）
 * @param Ki 积分增益（推荐0.01~0.5）
 */
void MahonyInit(AttitudeFilter *filter, float Kp, float Ki)
{
	filter->qw = 1.0f; // 初始四元数（单位四元数，无旋转）
	filter->qx = 0.0f;
	filter->qy = 0.0f;
	filter->qz = 0.0f;
	filter->exInt = 0.0f; // 积分误差初始化为0
	filter->eyInt = 0.0f;
	filter->ezInt = 0.0f;
	filter->Kp = Kp; // 设置增益
	filter->Ki = Ki;
}

/**
 * @brief Mahony算法更新（仅用加速度计+陀螺仪，偏航角会漂移）
 * @param filter 滤波器实例指针
 * @param data 传感器数据（加速度计需归一化，陀螺仪单位为rad/s）
 * @param dt 采样时间（单位：秒）
 */
void MahonyUpdateIMU(AttitudeFilter *filter, const SensorData *data, float dt)
{
	float ax = data->ax;
	float ay = data->ay;
	float az = data->az;
	float gx = data->gx;
	float gy = data->gy;
	float gz = data->gz;

	// 步骤1：归一化加速度计数据（确保输入已归一化可跳过，但保险起见再做一次）
	float norm = sqrtf(ax * ax + ay * ay + az * az);
	if (norm < 1e-6f)
		return; // 避免除零（传感器故障）
	ax /= norm;
	ay /= norm;
	az /= norm;

	// 步骤2：计算当前姿态下的理论重力方向（基于四元数）
	float vx = 2.0f * (filter->qx * filter->qz - filter->qw * filter->qy);
	float vy = 2.0f * (filter->qw * filter->qx + filter->qy * filter->qz);
	float vz = filter->qw * filter->qw - filter->qx * filter->qx - filter->qy * filter->qy + filter->qz * filter->qz;

	// 步骤3：计算误差（实测重力与理论重力的叉积）
	float ex = ay * vz - az * vy;
	float ey = az * vx - ax * vz;
	float ez = ax * vy - ay * vx;

	// 步骤4：积分误差（用于消除静态偏差）
	filter->exInt += filter->Ki * ex * dt;
	filter->eyInt += filter->Ki * ey * dt;
	filter->ezInt += filter->Ki * ez * dt;

	// 步骤5：用误差修正陀螺仪数据
	gx += filter->Kp * ex + filter->exInt;
	gy += filter->Kp * ey + filter->eyInt;
	gz += filter->Kp * ez + filter->ezInt;

	// 步骤6：更新四元数（解微分方程，欧拉积分）
	float q0Dot = 0.5f * (-filter->qx * gx - filter->qy * gy - filter->qz * gz);
	float q1Dot = 0.5f * (filter->qw * gx + filter->qy * gz - filter->qz * gy);
	float q2Dot = 0.5f * (filter->qw * gy - filter->qx * gz + filter->qz * gx);
	float q3Dot = 0.5f * (filter->qw * gz + filter->qx * gy - filter->qy * gx);

	filter->qw += q0Dot * dt;
	filter->qx += q1Dot * dt;
	filter->qy += q2Dot * dt;
	filter->qz += q3Dot * dt;

	// 步骤7：归一化四元数（保持单位长度）
	norm = sqrtf(filter->qw * filter->qw + filter->qx * filter->qx + filter->qy * filter->qy + filter->qz * filter->qz);
	if (norm < 1e-6f)
		return;
	filter->qw /= norm;
	filter->qx /= norm;
	filter->qy /= norm;
	filter->qz /= norm;
}

/**
 * @brief Mahony算法更新（带磁力计，修正偏航角漂移）
 * @param filter 滤波器实例指针
 * @param data 传感器数据（含磁力计，需归一化）
 * @param dt 采样时间（单位：秒）
 */
void MahonyUpdateAHRS(AttitudeFilter *filter, const SensorData *data, float dt)
{
	float ax = data->ax;
	float ay = data->ay;
	float az = data->az;
	float gx = data->gx;
	float gy = data->gy;
	float gz = data->gz;
	float mx = data->mx;
	float my = data->my;
	float mz = data->mz;

	// 归一化加速度计和磁力计
	float norm = sqrtf(ax * ax + ay * ay + az * az);
	if (norm < 1e-6f)
		return;
	ax /= norm;
	ay /= norm;
	az /= norm;

	norm = sqrtf(mx * mx + my * my + mz * mz);
	if (norm < 1e-6f)
		return;
	mx /= norm;
	my /= norm;
	mz /= norm;

	// 计算磁场在机体坐标系中的投影（消除倾斜影响）
	float hx = mx * (1 - 2 * filter->qy * filter->qy - 2 * filter->qz * filter->qz) + my * (2 * filter->qx * filter->qy - 2 * filter->qw * filter->qz) + mz * (2 * filter->qx * filter->qz + 2 * filter->qw * filter->qy);
	float hy = mx * (2 * filter->qx * filter->qy + 2 * filter->qw * filter->qz) + my * (1 - 2 * filter->qx * filter->qx - 2 * filter->qz * filter->qz) + mz * (2 * filter->qy * filter->qz - 2 * filter->qw * filter->qx);
	float bx = sqrtf(hx * hx + hy * hy); // 水平磁场分量
	float bz = mx * (2 * filter->qx * filter->qz - 2 * filter->qw * filter->qy) + my * (2 * filter->qy * filter->qz + 2 * filter->qw * filter->qx) + mz * (1 - 2 * filter->qx * filter->qx - 2 * filter->qy * filter->qy);

	// 理论重力方向（同IMU版）
	float vx = 2.0f * (filter->qx * filter->qz - filter->qw * filter->qy);
	float vy = 2.0f * (filter->qw * filter->qx + filter->qy * filter->qz);
	float vz = filter->qw * filter->qw - filter->qx * filter->qx - filter->qy * filter->qy + filter->qz * filter->qz;

	// 理论磁场方向
	float wx = 2 * bx * (0.5f - filter->qy * filter->qy - filter->qz * filter->qz) + 2 * bz * (filter->qx * filter->qz - filter->qw * filter->qy);
	float wy = 2 * bx * (filter->qx * filter->qy - filter->qw * filter->qz) + 2 * bz * (filter->qw * filter->qx + filter->qy * filter->qz);

	// 计算误差（重力+磁场）
	float ex = (ay * vz - az * vy) + (my * wx - mx * wy);
	float ey = (az * vx - ax * vz) + (mx * wy - my * wx);	  // 修正符号（原算法可能有笔误）
	float ez = (ax * vy - ay * vx) + (mx * wy - my * wx) * 0; // 磁场对Z轴误差影响较小，可忽略

	// 后续步骤同IMU版：积分误差→修正陀螺仪→更新四元数
	filter->exInt += filter->Ki * ex * dt;
	filter->eyInt += filter->Ki * ey * dt;
	filter->ezInt += filter->Ki * ez * dt;

	gx += filter->Kp * ex + filter->exInt;
	gy += filter->Kp * ey + filter->eyInt;
	gz += filter->Kp * ez + filter->ezInt;

	float q0Dot = 0.5f * (-filter->qx * gx - filter->qy * gy - filter->qz * gz);
	float q1Dot = 0.5f * (filter->qw * gx + filter->qy * gz - filter->qz * gy);
	float q2Dot = 0.5f * (filter->qw * gy - filter->qx * gz + filter->qz * gx);
	float q3Dot = 0.5f * (filter->qw * gz + filter->qx * gy - filter->qy * gx);

	filter->qw += q0Dot * dt;
	filter->qx += q1Dot * dt;
	filter->qy += q2Dot * dt;
	filter->qz += q3Dot * dt;

	norm = sqrtf(filter->qw * filter->qw + filter->qx * filter->qx + filter->qy * filter->qy + filter->qz * filter->qz);
	if (norm < 1e-6f)
		return;
	filter->qw /= norm;
	filter->qx /= norm;
	filter->qy /= norm;
	filter->qz /= norm;
}

// -------------------------- 四元数转欧拉角 --------------------------
/**
 * @brief 四元数转换为欧拉角（弧度）
 * @param filter 滤波器实例指针
 * @param angles 输出的欧拉角结构体指针
 */
void QuaternionToEuler(const AttitudeFilter *filter, EulerAngles *angles)
{
	const float qw = filter->qw;
	const float qx = filter->qx;
	const float qy = filter->qy;
	const float qz = filter->qz;

	// 滚转角（X轴）
	angles->roll = atan2f(2.0f * (qw * qx + qy * qz), 1.0f - 2.0f * (qx * qx + qy * qy)) * 180.0f / M_PI;

	// 俯仰角（Y轴）
	float sinp = 2.0f * (qw * qy - qz * qx);
	if (fabsf(sinp) >= 1.0f)
	{
		angles->pitch = copysignf(90.0f, sinp); // 限制在±90°
	}
	else
	{
		angles->pitch = asinf(sinp) * 180.0f / M_PI;
	}

	// 偏航角（Z轴）
	angles->yaw = atan2f(2.0f * (qw * qz + qx * qy), 1.0f - 2.0f * (qy * qy + qz * qz)) * 180.0f / M_PI;
}

// /**
//  * @brief 初始化Madgwick滤波器
//  * @param filter 滤波器实例指针
//  * @param beta 增益参数（推荐0.1~0.5，越小响应越慢但平滑）
//  */
// void MadgwickInit(AttitudeFilter *filter, float beta)
// {
// 	filter->qw = 1.0f;
// 	filter->qx = 0.0f;
// 	filter->qy = 0.0f;
// 	filter->qz = 0.0f;
// 	filter->exInt = 0.0f; // Madgwick不使用积分误差，此处复用变量存beta
// 	filter->eyInt = 0.0f;
// 	filter->ezInt = 0.0f;
// 	filter->Kp = beta; // 用Kp存储beta参数
// 	filter->Ki = 0.0f; // Madgwick通常不使用积分项
// }

// /**
//  * @brief Madgwick算法更新（仅用加速度计+陀螺仪）
//  * @param filter 滤波器实例指针
//  * @param data 传感器数据（加速度计需归一化，陀螺仪单位为rad/s）
//  * @param dt 采样时间（单位：秒）
//  */
// void MadgwickUpdateIMU(AttitudeFilter *filter, const SensorData *data, float dt)
// {
// 	float ax = data->ax;
// 	float ay = data->ay;
// 	float az = data->az;
// 	float gx = data->gx;
// 	float gy = data->gy;
// 	float gz = data->gz;
// 	float beta = filter->Kp;

// 	// 归一化加速度计
// 	float norm = sqrtf(ax * ax + ay * ay + az * az);
// 	if (norm < 1e-6f)
// 		return;
// 	ax /= norm;
// 	ay /= norm;
// 	az /= norm;

// 	// 计算四元数导数的误差项（基于梯度下降）
// 	float f1 = 2 * (filter->qx * filter->qz - filter->qw * filter->qy) - ax;
// 	float f2 = 2 * (filter->qw * filter->qx + filter->qy * filter->qz) - ay;
// 	float f3 = 2 * (0.5f - filter->qx * filter->qx - filter->qy * filter->qy) - az;

// 	// 误差项的雅各比矩阵
// 	float J11 = -2 * filter->qy;
// 	float J12 = 2 * filter->qz;
// 	float J13 = -2 * filter->qw;
// 	float J14 = 2 * filter->qx;
// 	float J21 = 2 * filter->qx;
// 	float J22 = 2 * filter->qw;
// 	float J23 = 2 * filter->qz;
// 	float J24 = 2 * filter->qy;
// 	float J31 = 0.0f;
// 	float J32 = -4 * filter->qx;
// 	float J33 = -4 * filter->qy;
// 	float J34 = 0.0f;

// 	// 梯度下降修正项
// 	float step0 = J11 * f1 + J21 * f2 + J31 * f3;
// 	float step1 = J12 * f1 + J22 * f2 + J32 * f3;
// 	float step2 = J13 * f1 + J23 * f2 + J33 * f3;
// 	float step3 = J14 * f1 + J24 * f2 + J34 * f3;

// 	// 归一化修正项
// 	norm = sqrtf(step0 * step0 + step1 * step1 + step2 * step2 + step3 * step3);
// 	if (norm < 1e-6f)
// 		return;
// 	step0 /= norm;
// 	step1 /= norm;
// 	step2 /= norm;
// 	step3 /= norm;

// 	// 用修正项更新四元数导数
// 	float q0Dot = 0.5f * (-filter->qx * gx - filter->qy * gy - filter->qz * gz) - beta * step0;
// 	float q1Dot = 0.5f * (filter->qw * gx + filter->qy * gz - filter->qz * gy) - beta * step1;
// 	float q2Dot = 0.5f * (filter->qw * gy - filter->qx * gz + filter->qz * gx) - beta * step2;
// 	float q3Dot = 0.5f * (filter->qw * gz + filter->qx * gy - filter->qy * gx) - beta * step3;

// 	// 积分更新四元数
// 	filter->qw += q0Dot * dt;
// 	filter->qx += q1Dot * dt;
// 	filter->qy += q2Dot * dt;
// 	filter->qz += q3Dot * dt;

// 	// 归一化
// 	norm = sqrtf(filter->qw * filter->qw + filter->qx * filter->qx + filter->qy * filter->qy + filter->qz * filter->qz);
// 	if (norm < 1e-6f)
// 		return;
// 	filter->qw /= norm;
// 	filter->qx /= norm;
// 	filter->qy /= norm;
// 	filter->qz /= norm;
// }